﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;

namespace Brain
{
    public static class TriangleUtil
    {
        public static Vector3 Center(Vector3 v1, Vector3 v2, Vector3 v3)
        {
            return (v1 + v2 + v3) / 3f;
        }

        public static void ConvertToIndices(Vector3[] vertices, out Vector3[] newVertices,
          out int[] indices)
        {
            List<Vector3> news = new List<Vector3>();
            List<int> indi = new List<int>();

            int index = 0;
            for (int i = 0; i < vertices.Length; i++)
            {
                Vector3 v = vertices[i];
                if (news.Contains(v))
                {
                    indi.Add(news.IndexOf(v));
                }
                else
                {
                    news.Add(v);
                    indi.Add(index);
                    index++;
                }

            }

            newVertices = news.ToArray();
            indices = indi.ToArray();
        }

        public static void ConvertToIndices(Vector3[] vertices, out Vector3[] newVertices,
            out short[] indices)
        {
            List<Vector3> news = new List<Vector3>();
            List<short> indi = new List<short>();

            int index = 0;
            for (int i = 0; i < vertices.Length; i++)
            {
                Vector3 v = vertices[i];
                if (news.Contains(v))
                {
                    indi.Add((short)news.IndexOf(v));
                }
                else
                {
                    news.Add(v);
                    indi.Add((short)index);
                    index++;
                }

            }

            newVertices = news.ToArray();
            indices = indi.ToArray();
        }

        public static List<Vector3> Triangulate(List<Vector3> quads)
        {
            List<Vector3> triangles = new List<Vector3>();

            for (int i = 0; i < quads.Count - 3; i += 4)
            {
                int realI = i / 4;

                Vector3 q1 = quads[i];
                Vector3 q2 = quads[i + 1];
                Vector3 q3 = quads[i + 2];
                Vector3 q4 = quads[i + 3];

                triangles.Add(q1);
                triangles.Add(q2);
                triangles.Add(q3);

                triangles.Add(q1);
                triangles.Add(q3);
                triangles.Add(q4);
            }

            return triangles;
        }

        /// <summary>
        /// Tesselate the Triangles. This method will divide every triangle
        /// in 4 equal parts.
        /// </summary>
        /// <param name="startTris"></param>
        /// <returns></returns>
        public static Triangle[] Tesselate(Triangle[] startTris)
        {
            Triangle[] triangles = new Triangle[startTris.Length * 4];

            int index = 0;
            for (int i = 0; i < startTris.Length; i++)
            {
                Triangle tri = startTris[i];

                // Triangle 1
                Vector3 tri1v1 = tri.Vertex1;
                Vector3 tri1v2 = Vector3.Lerp(tri.Vertex1, tri.Vertex2, 0.5f);
                Vector3 tri1v3 = Vector3.Lerp(tri.Vertex1, tri.Vertex3, 0.5f);
                Triangle tri1 = triangles[index] = new Triangle() { Vertex1 = tri1v1, Vertex2 = tri1v2, Vertex3 = tri1v3 };
                index++;
                tri1.Normal = tri.Normal;

                // Triangle 2
                Vector3 tri2v1 = Vector3.Lerp(tri.Vertex1, tri.Vertex2, 0.5f);
                Vector3 tri2v2 = tri.Vertex2;
                Vector3 tri2v3 = Vector3.Lerp(tri.Vertex2, tri.Vertex3, 0.5f);
                Triangle tri2 = triangles[index] = new Triangle() { Vertex1 = tri2v1, Vertex2 = tri2v2, Vertex3 = tri2v3 };
                index++;
                tri2.Normal = tri.Normal;

                // Triangle 3
                Vector3 tri3v1 = Vector3.Lerp(tri.Vertex1, tri.Vertex2, 0.5f);
                Vector3 tri3v2 = Vector3.Lerp(tri.Vertex1, tri.Vertex3, 0.5f);
                Vector3 tri3v3 = Vector3.Lerp(tri.Vertex2, tri.Vertex3, 0.5f);
                Triangle tri3 = triangles[index] = new Triangle() { Vertex1 = tri3v1, Vertex2 = tri3v2, Vertex3 = tri3v3 };
                index++;
                tri3.Normal = tri.Normal;

                // Triangle 4
                Vector3 tri4v1 = Vector3.Lerp(tri.Vertex1, tri.Vertex3, 0.5f);
                Vector3 tri4v2 = tri.Vertex3;
                Vector3 tri4v3 = Vector3.Lerp(tri.Vertex2, tri.Vertex3, 0.5f);
                Triangle tri4 = triangles[index] = new Triangle() { Vertex1 = tri4v1, Vertex2 = tri4v2, Vertex3 = tri4v3 };
                index++;
                tri4.Normal = tri.Normal;
            }

            return triangles;
        }

        /// <summary>
        /// Tesselate the Triangle. This method will divide the triangle
        /// in 4 equal parts.
        /// </summary>
        /// <param name="startTris"></param>
        /// <returns></returns>
        public static void Tesselate(Triangle tri, out Triangle tri1, out Triangle tri2,
            out Triangle tri3, out Triangle tri4)
        {
            // Triangle 1
            Vector3 tri1v1 = tri.Vertex1;
            Vector3 tri1v2 = Vector3.Lerp(tri.Vertex1, tri.Vertex2, 0.5f);
            Vector3 tri1v3 = Vector3.Lerp(tri.Vertex1, tri.Vertex3, 0.5f);
            tri1 = new Triangle() { Vertex1 = tri1v1, Vertex2 = tri1v2, Vertex3 = tri1v3 };
            tri1.Normal = tri.Normal;

            // Triangle 2
            Vector3 tri2v1 = Vector3.Lerp(tri.Vertex1, tri.Vertex2, 0.5f);
            Vector3 tri2v2 = tri.Vertex2;
            Vector3 tri2v3 = Vector3.Lerp(tri.Vertex2, tri.Vertex3, 0.5f);
            tri2 = new Triangle() { Vertex1 = tri2v1, Vertex2 = tri2v2, Vertex3 = tri2v3 };
            tri2.Normal = tri.Normal;

            // Triangle 3
            Vector3 tri3v1 = Vector3.Lerp(tri.Vertex1, tri.Vertex2, 0.5f);
            Vector3 tri3v2 = Vector3.Lerp(tri.Vertex1, tri.Vertex3, 0.5f);
            Vector3 tri3v3 = Vector3.Lerp(tri.Vertex2, tri.Vertex3, 0.5f);
            tri3 = new Triangle() { Vertex1 = tri3v1, Vertex2 = tri3v2, Vertex3 = tri3v3 };
            tri3.Normal = tri.Normal;

            // Triangle 4
            Vector3 tri4v1 = Vector3.Lerp(tri.Vertex1, tri.Vertex3, 0.5f);
            Vector3 tri4v2 = tri.Vertex3;
            Vector3 tri4v3 = Vector3.Lerp(tri.Vertex2, tri.Vertex3, 0.5f);
            tri4 = new Triangle() { Vertex1 = tri4v1, Vertex2 = tri4v2, Vertex3 = tri4v3 };
            tri4.Normal = tri.Normal;
        }


        /// <summary>
        /// Shows how much vertices between these Triangles are equal
        /// </summary>
        /// <param name="tri1"></param>
        /// <param name="tri2"></param>
        /// <returns></returns>
        public static ushort HowManyEqual(Triangle tri1, Triangle tri2)
        {
            ushort eq = 0;

            if (tri1.Vertex1 == tri2.Vertex1)
            {
                eq++;
            }
            if (tri1.Vertex1 == tri2.Vertex2)
            {
                eq++;
            }
            if (tri1.Vertex1 == tri2.Vertex3)
            {
                eq++;
            }
            if (tri1.Vertex2 == tri2.Vertex1)
            {
                eq++;
            }
            if (tri1.Vertex2 == tri2.Vertex2)
            {
                eq++;
            }
            if (tri1.Vertex2 == tri2.Vertex3)
            {
                eq++;
            }
            if (tri1.Vertex3 == tri2.Vertex1)
            {
                eq++;
            }
            if (tri1.Vertex3 == tri2.Vertex2)
            {
                eq++;
            }
            if (tri1.Vertex3 == tri2.Vertex3)
            {
                eq++;
            }

            return eq;
        }

        public static void Normal(VertexPositionNormalTexture[] vertices, int[] indices)
        {
            for (int i = 0; i < indices.Length - 2; i += 3)
            {
                int index1 = indices[i];
                int index2 = indices[i + 1];
                int index3 = indices[i + 2];

                VertexPositionNormalTexture v1 = vertices[index1];
                VertexPositionNormalTexture v2 = vertices[index2];
                VertexPositionNormalTexture v3 = vertices[index3];

                Vector3 normal = Normal(v1.Position, v2.Position, v3.Position);
                vertices[index1].Normal = normal;
                vertices[index2].Normal = normal;
                vertices[index3].Normal = normal;
            }
        }
        public static void Normal(VertexPositionNormalTexture[] vertices, short[] indices)
        {
            for (int i = 0; i < indices.Length - 2; i += 3)
            {
                short index1 = indices[i];
                short index2 = indices[i + 1];
                short index3 = indices[i + 2];

                VertexPositionNormalTexture v1 = vertices[index1];
                VertexPositionNormalTexture v2 = vertices[index2];
                VertexPositionNormalTexture v3 = vertices[index3];

                Vector3 normal = Normal(v1.Position, v2.Position, v3.Position);
                vertices[index1].Normal = normal;
                vertices[index2].Normal = normal;
                vertices[index3].Normal = normal;
            }
        }

        /// <summary>
        /// Calculates the Normal of a Triangle. Adapted from Riemers
        /// </summary>
        /// <param name="vertex1"></param>
        /// <param name="vertex2"></param>
        /// <param name="vertex3"></param>
        /// <returns></returns>
        public static Vector3 Normal(Vector3 vertex1, Vector3 vertex2, Vector3 vertex3)
        {
            Vector3 vec1 = vertex2 - vertex1;
            Vector3 vec2 = vertex2 - vertex3;
            Vector3 normal = Vector3.Cross(vec1, vec2);
            if (normal != Vector3.Zero)
            {
                normal.Normalize();
            }
            return normal;
        }


        /// <summary>
        /// Calculates the Normal of a Triangle. Adapted from Riemers
        /// </summary>
        /// <param name="vertex1"></param>
        /// <param name="vertex2"></param>
        /// <param name="vertex3"></param>
        /// <returns></returns>
        public static Vector3 Normal(Triangle triangle)
        {
            return Normal(triangle.Vertex1, triangle.Vertex2, triangle.Vertex3);
        }

        /// <summary>
        /// Gets all the Vertices, including the Indexed ones as an 1 array of vertices
        /// </summary>
        /// <param name="vertices"></param>
        /// <param name="indices"></param>
        /// <returns></returns>
        public static Vector3[] GetAllVertices(Vector3[] vertices, int[] indices)
        {
            Vector3[] arrei = new Vector3[indices.Length];
            for (int i = 0; i < indices.Length; i++)
            {
                arrei[i] = vertices[indices[i]];
            }
            return arrei;
        }

#if !SILVERLIGHT && !MONOGAME
        /// <summary>
        /// Gets an array of vertices and indices from the provided model. - From bEPU
        /// </summary>
        /// <param name="collisionModel">Model to use for the collision shape.</param>
        /// <param name="vertices">Compiled set of vertices from the model.</param>
        /// <param name="indices">Compiled set of indices from the model.</param>
        public static void GetVerticesAndIndicesFromModel(Model collisionModel, out Vector3[] vertices, out int[] indices)
        {
            var verticesList = new List<Vector3>();
            var indicesList = new List<int>();
            var transforms = new Matrix[collisionModel.Bones.Count];
            collisionModel.CopyAbsoluteBoneTransformsTo(transforms);

            Matrix transform;
            foreach (ModelMesh mesh in collisionModel.Meshes)
            {
                if (mesh.ParentBone != null)
                    transform = transforms[mesh.ParentBone.Index];
                else
                    transform = Matrix.Identity;
                AddMesh(mesh, transform, verticesList, indicesList);
            }

            vertices = verticesList.ToArray();
            indices = indicesList.ToArray();


        }

        /// <summary>
        /// Adds a mesh's vertices and indices to the given lists. - From bEPU
        /// </summary>
        /// <param name="collisionModelMesh">Model to use for the collision shape.</param>
        /// <param name="transform">Transform to apply to the mesh.</param>
        /// <param name="vertices">List to receive vertices from the mesh.</param>
        /// <param name="indices">List to receive indices from the mesh.</param>
        public static void AddMesh(ModelMesh collisionModelMesh, Matrix transform, List<Vector3> vertices, IList<int> indices)
        {
            foreach (ModelMeshPart meshPart in collisionModelMesh.MeshParts)
            {
                int startIndex = vertices.Count;
                var meshPartVertices = new Vector3[meshPart.NumVertices];
                //Grab position data from the mesh part.
                int stride = meshPart.VertexBuffer.VertexDeclaration.VertexStride;
                meshPart.VertexBuffer.GetData(
                        meshPart.VertexOffset * stride,
                        meshPartVertices,
                        0,
                        meshPart.NumVertices,
                        stride);

                //Transform it so its vertices are located in the model's space as opposed to mesh part space.
                Vector3.Transform(meshPartVertices, ref transform, meshPartVertices);
                vertices.AddRange(meshPartVertices);

                if (meshPart.IndexBuffer.IndexElementSize == IndexElementSize.ThirtyTwoBits)
                {
                    var meshIndices = new int[meshPart.PrimitiveCount * 3];
                    meshPart.IndexBuffer.GetData(meshPart.StartIndex * 4, meshIndices, 0, meshPart.PrimitiveCount * 3);
                    for (int k = 0; k < meshIndices.Length; k++)
                    {
                        indices.Add(startIndex + meshIndices[k]);
                    }
                }
                else
                {
                    var meshIndices = new ushort[meshPart.PrimitiveCount * 3];
                    meshPart.IndexBuffer.GetData(meshPart.StartIndex * 2, meshIndices, 0, meshPart.PrimitiveCount * 3);
                    for (int k = 0; k < meshIndices.Length; k++)
                    {
                        indices.Add(startIndex + meshIndices[k]);
                    }


                }
            }
        }
#endif
    }
}
